package com.guazi.trackingtest;

import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.support.annotation.Nullable;
import android.widget.TextView;
import android.widget.Toast;

import com.guazi.trackingtest.filter.Kalman;
import com.kircherelectronics.fsensor.filter.averaging.MeanFilter;
import com.kircherelectronics.fsensor.filter.gyroscope.fusion.kalman.OrientationFusedKalman;
import com.kircherelectronics.fsensor.util.rotation.RotationUtil;

import java.io.IOException;

/**
 * Created by lqy on 2018/3/30.
 */

public class TestActivity extends Activity implements SensorEventListener {

    public static final int STATIC_DRIFT_COUNT = 300;

    TextView mTextView;

    Sensor mGyr;
    Sensor mAcc;
    Sensor mMag;

    SensorManager mManager;

    double mX = 0;
    double mY = 0;
    double mZ = 0;

    private Sender mSender;

    private int staticDriftCount = 0;

    private Kalman mKalman = new Kalman();

    @Override
    protected void onCreate(@Nullable Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_test);

        mTextView = findViewById(R.id.activity_test_text);

        mManager = (SensorManager) getSystemService(SENSOR_SERVICE);
        assert mManager != null;

        mGyr = mManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
        mAcc = mManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        mMag = mManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);

        mManager.registerListener(this, mGyr, SensorManager.SENSOR_DELAY_GAME);
        mManager.registerListener(this, mAcc, SensorManager.SENSOR_DELAY_GAME);
        mManager.registerListener(this, mMag, SensorManager.SENSOR_DELAY_GAME);

        initJsSender();
    }

    @Override
    protected void onResume() {
        super.onResume();
        if(orientationKalmanFusion != null) {
            orientationKalmanFusion.startFusion();
        }
    }

    @Override
    protected void onDestroy() {
        super.onDestroy();
        mManager.unregisterListener(this);
        if (mSender != null) {
            mSender.stop();
        }
        orientationKalmanFusion.stopFusion();
    }

    private float[] fusedOrientation = new float[3];
    private float[] acceleration = new float[4];
    private float[] magnetic = new float[3];
    private float[] rotation = new float[3];

    private boolean hasAcceleration = false;
    private boolean hasMagnetic = false;

    private OrientationFusedKalman orientationKalmanFusion = new OrientationFusedKalman();
    private MeanFilter meanFilter = new MeanFilter();

    @Override
    public void onSensorChanged(SensorEvent event) {
        if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
            System.arraycopy(event.values, 0, rotation, 0, event.values.length);

            if (staticDriftCount < STATIC_DRIFT_COUNT) {
                mX += event.values[0];
                mY += event.values[1];
                mZ += event.values[2];
                staticDriftCount++;
                mTextView.setText("角速度累积：\n" + "x：" + mX + "\ny：" + mY + "\nz：" + mZ);
            } else if (staticDriftCount == STATIC_DRIFT_COUNT) {
                mX = mX / STATIC_DRIFT_COUNT;
                mY = mY / STATIC_DRIFT_COUNT;
                mZ = mZ / STATIC_DRIFT_COUNT;
                mTextView.setText(mTextView.getText() + "\n平均静态漂移：\n" + "x：" + mX + "\ny：" + mY + "\nz：" + mZ);
                double[] array = {mX * 10, mY * 10, mZ * 10};
//                double[] array = {0.05, 0.05, 0.05};
                mKalman.setNoise(array);
                staticDriftCount++;
            } else {
                mSender.addPackData(new PackData(0, 0, event.values[0], event.values[1], event.values[2]));
                mSender.addPackData(mKalman.filter(new PackData(1, 0, event.values[0], event.values[1], event.values[2])));
                if (!orientationKalmanFusion.isBaseOrientationSet()) {
                    if (hasAcceleration && hasMagnetic) {
                        orientationKalmanFusion.setBaseOrientation(RotationUtil.getOrientationQuaternionFromAccelerationMagnetic(acceleration, magnetic));
                    }
                } else {
                    fusedOrientation = orientationKalmanFusion.calculateFusedOrientation(rotation, event.timestamp, acceleration, magnetic);
                }

                mSender.addPackData(new PackData(2, 0, fusedOrientation[0], fusedOrientation[1], fusedOrientation[2]));
                fusedOrientation = meanFilter.filter(fusedOrientation);
                mSender.addPackData(new PackData(3, 0, fusedOrientation[0], fusedOrientation[1], fusedOrientation[2]));
            }
        } else if(event.sensor.getType() == Sensor.TYPE_ACCELEROMETER){
            System.arraycopy(event.values, 0, acceleration, 0, event.values.length);
            hasAcceleration = true;
        } else if(event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD){
            // Android reuses events, so you probably want a copy
            System.arraycopy(event.values, 0, magnetic, 0, event.values.length);
            hasMagnetic = true;
        }
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {

    }

    private void initJsSender() {
        if (mSender == null) {
            mSender = new Sender();
            mSender.start();
            Toast.makeText(this, "sender初始化成功", Toast.LENGTH_SHORT).show();
            new Thread(new Runnable() {
                @Override
                public void run() {
                    while (true) {
                        try {
                            mSender.sendDataToJs();
                        } catch (InterruptedException e) {
                            e.printStackTrace();
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                }
            }).start();
        }
    }
}
